#include "rtthread.h"
#include "motor_thread.h"
#include "tim.h"
#include "encode.h"
#include "drv_pwmMotors.h"
#include "AdgustSpeed.h"
#include "Motor_Control.h"
#include "Inc_Encoder.h"
#include "MotorHall.h"
#include "Motor_CurrentSense.h"

#include "HalPWM.h"
#include "SimpleFOC.h"
#include "UserMotor.h"
#include "config.h"
ZY_motor_drive_t motor_handle;
ZY_FOC_Move_t zy_speed;
ZY_Control_t  control_target;
extern BLDCMotor_t     simple_motor;
extern int inc_encode_acc;


mixer_speed_s mixer_speed = {
	.rotor.index = 0,
	.rotor.size = 9,
	.init_status = false,
	.motor_drive = &motor_handle};
float Speed_Adgust(ZY_FOC_Move_t *motor_speed);

void User_ZY_Move_Init()
{
	zy_speed.velocity_pid.kp=1;
	zy_speed.velocity_pid.ki=0.5;
	zy_speed.velocity_pid.kd=0.6;
	zy_speed.target_speed=0;
	zy_speed.current_speed=0;
	zy_speed.velocity_pid.err=0;
	zy_speed.velocity_pid.err_last=0;
	zy_speed.enable=1;
	zy_speed.motor_drive=&motor_handle;
}
/***************************************************
* 功能：使能电机刹车
* 参数：
*         enbale：刹车状态
***************************************************/
void SetMotorStop(bool enable)
{
	motor_handle.EnableStop = enable;
}
void ZY_SetMotor_torque(float torque)
{
	control_target.model=ZY_Model_torque;
	control_target.value=torque;
}
/****************************************************
 * 功能：设置电机速度
 * 参数：
 *       speed：电机速度
 * 返回：无
 * *************************************************/
void ZY_SetMotorSpeed(float speed)
{
	//zy_speed.target_speed = ((speed / 60.0)*0.02) * ((float)motor_handle.encoder.value.Resolution+1.0); //目标速度
	control_target.value=((speed / 60.0)*0.02) * ((float)motor_handle.encoder.value.Resolution+1.0);
	control_target.model=ZY_Model_Velocity;
}
float ZY_GetMotorSpeed(void)
{
	return ((float)zy_speed.current_speed/0.02)*60.0/((float)motor_handle.encoder.value.Resolution+1.0);
}
float ZY_GetMotorAngle(void)
{
	return (float)inc_encode_acc/4096.0*TWOPI;
}
/***************************************************
 * 功能：获取电机运行目标速度
 * 参数：
 *         无
 * 返回：当前电机目标速度
 * ************************************************/
float GetMotorTarget(void)
{
	return -(float)zy_speed.target_speed;
}
/***************************************************
 * 功能：获取电机速度环PID
 * 参数：
 *         无
 * 返回：当前电机目标速度
 * ************************************************/
PID_t ZY_GetPID_Velocity(void)
{
	PID_t pid;
	pid.p=zy_speed.velocity_pid.kp;
	pid.i=zy_speed.velocity_pid.ki;
	pid.d=zy_speed.velocity_pid.kd;
	return pid;
}

/***************************************************
 * 功能：获取电机位置环PID
 * 参数：
 *         无
 * 返回：当前电机目标速度
 * ************************************************/
PID_t ZY_GetPID_Angle(void)
{
	PID_t pid;
	pid.p=0;
	pid.i=0;
	pid.d=0;
	return pid;
}
/***************************************************
 * 功能：获取电机电流环PID
 * 参数：
 *         无
 * 返回：当前电机目标速度
 * ************************************************/
PID_t ZY_GetPID_Current(void)
{
	PID_t pid;
	pid.p=0;
	pid.i=0;
	pid.d=0;
	return pid;
}
/***************************************************
 * 功能：设置电机速度环PID
 * 参数：
 *         无
 * ************************************************/
void ZY_SetPID_Velocity(PID_t pid)
{
	zy_speed.velocity_pid.kp=pid.p;
	zy_speed.velocity_pid.ki=pid.i;
	zy_speed.velocity_pid.kd=pid.d;
}
/**************************************************
* 功能：设置电机编码器零点偏移
* 参数：无
* 返回：无
**************************************************/
void SetMotorEncodeZero(void)
{
	zy_speed.enable = false;
	ZY_Set_Power_Motor(&motor_handle, 0, 20);
	rt_thread_mdelay(1000);
	Read_Encode(&motor_handle.encoder);
	motor_handle.encoder.offset_value=motor_handle.encoder.value.encode_value;
	rt_thread_mdelay(500);
	ZY_Set_Power_Motor(&motor_handle, 0, 0);
	//motor_driver1.encoder.offset_value = motor_driver1.encoder.value->encode_value;
	zy_speed.enable = true;
}
/// @brief 启动电机PWM
/// @param  
void Start_MotorPWM(void)
{
	HAL_TIM_Base_Start_IT(&htim1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_1);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_2);
	HAL_TIM_PWM_Start(&htim1,TIM_CHANNEL_3);
	HAL_TIMEx_PWMN_Start(&htim1,TIM_CHANNEL_3);
}

float motor_speed=500;
/// @brief 电机入口线程
/// @param paremeter 
void motor_thread_entry(void *paremeter)
{
	HAL_GPIO_WritePin(Driver_EN_GPIO_Port,Driver_EN_Pin,GPIO_PIN_RESET);
	Mixer_Init(&mixer_speed);
	User_ZY_Move_Init();
	Inc_Encoder_Init();
	MotorHall_Enable();
	Encoder_Function(&motor_handle.encoder,Inc_Encode_Read);   //注册编码器读数
  Encoder_Registration(&motor_handle.encoder,0,4095,12);     //注册编码器
	Read_Encode(&motor_handle.encoder);     //更新编码器值
	//MotorControl_Init();   //脉冲方向控制初始化
	rt_thread_mdelay(2000);
	//SetSpeedPid(&model_info_handle.model_info.speed_pid);   //加载PID参数
	Start_CurrentCalibration();
	//初始化电机
	ZY_initSinArray();			  //初始化正弦计算公式
	ZY_Motor_Init(&motor_handle,4,95,Start_MotorPWM,Set_MotorPWM); //初始化电机
	rt_thread_mdelay(500);
	ZY_Set_Power_Motor(&motor_handle, 0, 0);
	HAL_GPIO_WritePin(Driver_EN_GPIO_Port,Driver_EN_Pin,GPIO_PIN_SET);
	SetMotorEncodeZero();
	//set_TargetSpeed(&mixer_speed,63);
	//SetMotorSpeed(1000);
	motor_handle.motor_power=0;
	Init_CurrentSence();
	Init_Simple();
	float angle=0;
	HAL_TIM_Base_Start_IT(&htim5);      //
	//ZY_SetMotorSpeed(100);
	ZY_SetMotor_torque(5);
	while (1)
	{
		//SetMotorSpeed(motor_speed);
//		angle+=0.01;
//		setPhaseVoltage(&simple_motor,3, 0,  angle);
		//Read_Encode(&motor_handle.encoder);
		//p_m+=0.1;
		//ZY_Set_Power_Motor(&motor_handle, p_m, 10);
		rt_thread_mdelay(10);
	}
}

uint16_t s_i=0;
void ZY_FOC_loop()
{
	Read_Encode(&motor_handle.encoder);
	if (zy_speed.enable)
	{
		//获取电机编码器positio
		ZY_Motor_RotorPosition(&motor_handle);
		s_i++;
		if(s_i==2000)
		{
			s_i=0;
			//getRotor(&mixer_speed, motor_handle.encoder.user_value);
			// mixer_speed.Target_Delta = (float)motor_speed1.target_speed;
			Motor_Speed(&zy_speed,&mixer_speed,control_target);
		}
	}
}
/// @brief 外部引脚中断回调
/// @param GPIO_Pin 
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
	if(GPIO_Pin==E_I_Pin)
	{
		//Clear_Inc_Encoder();
	}
}